package 局部路径规划器;

import 图表库.Jpolt;
import 图表库.JpoltCurve;
import 曲线多项式.Spline2D;
import 曲线多项式.quartic_polynomial;
import 曲线多项式.quintic_polynomial;

import java.awt.*;
import java.math.BigDecimal;
import java.util.Vector;
import java.util.HashSet;
public class frenetPlan {
    public frenetPlan(){}
    public Vector<frenetPath> planning(Spline2D spline, double s0,double c_speed,double c_d,double c_dd,double c_ddd,Vector<point> ob){
        Vector<frenetPath> Paths = calc_paths(c_speed,c_d,c_dd,c_ddd,s0);
        Paths = calc_global_paths(Paths,spline);
        Paths = check_paths(Paths,ob);
        return Paths;
    }
    public frenetPath best_path(Vector<frenetPath> Paths){
        frenetPath best_fp = null;
        // 使用优先队列优化
        double minCost = Double.POSITIVE_INFINITY; // 初始化为正无穷大
        for (frenetPath fp:Paths) {
            if (minCost >= fp.cf) {
                minCost = fp.cf;
                best_fp = fp;
            }
        }
        //System.out.println(best_fp.cf);
        return best_fp;
    }


    public Vector<frenetPath> calc_paths(double c_speed,double c_d,double c_dd,double c_ddd,double s0){ // 求解可能路径集
        Vector<frenetPath> paths = new Vector<>(); // 存储全部路径集
        Vector<Double> roadWidth = range(-frenetConst.MAX_ROAD_WIDTH,frenetConst.MAX_ROAD_WIDTH,frenetConst.D_ROAD_W);
        Vector<Double> T = range(frenetConst.MINT,frenetConst.MAXT,frenetConst.DT);
        Vector<Double> V = range(frenetConst.TARGET_SPEED - frenetConst.D_T_S*frenetConst.N_S_SAMPLE,
                                 frenetConst.TARGET_SPEED + frenetConst.D_T_S*frenetConst.N_S_SAMPLE,
                                 frenetConst.D_T_S);
        BigDecimal Jp = new BigDecimal(String.valueOf(0));
        BigDecimal Js = new BigDecimal(String.valueOf(0));
        for (double di:roadWidth) {  // di 列表包含 d轴从最左（-7 m）侧到最右侧（7 m），每 1 米生成一个di 共 15 个
            for (double ti:T) {  // 时间列表： 从 4 开始，到 5 结束，每隔 0.2 生成一个 Ti，共 5个：4.0、4.2、4.4、4.6、4.8
                frenetPath fp = new frenetPath();
                quintic_polynomial lat_qp = new quintic_polynomial(c_d,c_dd,c_ddd,di,0,0,ti); // 生成五次多项式类
                fp.t = range(0,ti,frenetConst.DT);
                // 初始化  BigDecimal.ZERO -----------------------------------------------------------
                Jp = BigDecimal.ZERO;
                Js = BigDecimal.ZERO;
                for (double x:fp.t) {
                    fp.d.add(lat_qp.calc_point(x)); // 求 frenet 中 D 的值
                    fp.d_d.add(lat_qp.calc_first_derivative(x));
                    fp.d_dd.add(lat_qp.calc_second_derivative(x));
                    fp.d_ddd.add(lat_qp.calc_third_derivative(x));
                    Jp = Jp.add(new BigDecimal(String.valueOf(fp.d_ddd.lastElement())).pow(2));
                }
                // 求 frenet 中 S 的值
                for (double vi: V) {  // 循环次数与目标速度差值有关
                    quartic_polynomial lon_qp = new quartic_polynomial(s0, c_speed, 0.0,vi,0,ti); // 生成四次多项式类
                    frenetPath cl_fp  = new frenetPath(fp.t, fp.d, fp.d_d, fp.d_dd, fp.d_ddd);
                    for (double x:fp.t) {
                        cl_fp.s.add(lon_qp.calc_point(x));
                        cl_fp.s_d.add(lon_qp.calc_first_derivative(x));
                        cl_fp.s_dd.add(lon_qp.calc_second_derivative(x));
                        cl_fp.s_ddd.add(lon_qp.calc_third_derivative(x));
                        Js = Js.add(new BigDecimal(String.valueOf(cl_fp.s_ddd.lastElement())).pow(2));
                    }
                    BigDecimal ds =  new BigDecimal(String.valueOf(frenetConst.TARGET_SPEED)).subtract(new BigDecimal(String.valueOf(cl_fp.s_d.lastElement())));
                    ds = ds.pow(2);
                    // 计算代价
                    cl_fp.cd = frenetConst.KJ*Jp.doubleValue() + frenetConst.KT*ti + frenetConst.KD*Math.pow(cl_fp.d.lastElement(),2) ;
                    cl_fp.cv = frenetConst.KJ*Js.doubleValue() + frenetConst.KT*ti + frenetConst.KD* ds.doubleValue();
                    cl_fp.cf = frenetConst.KLAT*cl_fp.cd + frenetConst.KLON*cl_fp.cv;
                    paths.add(cl_fp);
                }
            }
        }
        return paths;
    }
    public Vector<frenetPath> calc_global_paths( Vector<frenetPath> paths,Spline2D spline){
        double ix,iy,iyaw,di,fx,fy;
        for (frenetPath fp:paths) {
            // 计算笛卡尔 x,y 坐标
            for (int i=0;i<fp.s.size();i++){
                ix = spline.calc_positionX(fp.s.get(i));
                iy = spline.calc_positionY(fp.s.get(i));
                iyaw = spline.calc_yaw(fp.s.get(i));
                di = fp.d.get(i);
                if (ix == Double.POSITIVE_INFINITY|| iy == Double.POSITIVE_INFINITY|| iyaw == Double.POSITIVE_INFINITY|| di == Double.POSITIVE_INFINITY){
                    break;
                }
                fx = ix + di * Math.cos(iyaw+Math.PI/2);
                fy = iy + di * Math.sin(iyaw+Math.PI/2);
                fp.x.add(fx);
                fp.y.add(fy);
                fp.position_set.add(new point(fx,fy));
            }
            // 计算 yaw and ds
            for (int i = 0; i < fp.x.size()-1; i++) {
                fp.yaw.add(Math.atan2(fp.y.get(i+1)-fp.y.get(i),fp.x.get(i+1)-fp.x.get(i)));
                fp.ds.add(Math.sqrt(Math.pow(fp.x.get(i+1)-fp.x.get(i),2)+Math.pow(fp.y.get(i+1)-fp.y.get(i),2)));
            }
            for (int i = 0; i < fp.yaw.size()-1; i++) {
                fp.c.add( (fp.yaw.get(i+1)-fp.yaw.get(i)) / fp.ds.get(i));
            }
        }
        return paths;
    }
    public Vector<frenetPath> check_paths(Vector<frenetPath> paths,Vector<point> ob){
        Vector<frenetPath> oK_paths = new Vector<>();
        for (frenetPath fp:paths) {
            if (is_Over(fp,ob)){
                continue;
            }
            oK_paths.add(fp);
        }
        return oK_paths;
    }
    public boolean is_Over(frenetPath fp,Vector<point> ob){
        for (double v:fp.s_d) {
            if (v > frenetConst.MAX_SPEED) {
                return true;
            }
        }
        for (double a:fp.s_dd) {
            if (a > frenetConst.MAX_ACCEL) {
                return true;
            }
        }
        for (double c:fp.c) {
            if (c > frenetConst.MAX_CURVATURE) {
                return true;
            }
        }
        // 检查是否与障碍物发生碰撞  哈希表优化
        double temp;
        for (int i = 0; i < ob.size(); i++) {
            for (int j = 0; j < fp.x.size(); j++) {
                temp = Math.pow((fp.x.get(j) - ob.get(i).x),2) + Math.pow((fp.y.get(j) - ob.get(i).y),2);
                if (temp < Math.pow(frenetConst.ROBOT_RADIUS,2)) {
                    return true;
                }
            }
        }
        // 检查是否与障碍物发生碰撞
        return false;
    }
    public Vector<Double> range(double min,double max,double unit){  //生成从 min 到 max 的数据，步长为 unit
        Vector<Double> temp = new Vector<>();
        // 采用 BigDecimal 数据类型，避免小数的精度丢失
        for (BigDecimal i = new BigDecimal(String.valueOf(min)); i.compareTo(new BigDecimal(String.valueOf(max))) <= 0;) {
            temp.add(i.doubleValue());
            i = i.add(new BigDecimal(String.valueOf(unit)));
        }
        // 结果可能出错******************************************************************
//        if (temp.lastElement() < max) {
//            temp.add(max);
//        }
        return temp;
    }
}
class Main{

    public static void main(String[] args) {
        double[] x = {0.0, 10.0, 20.5, 35.0, 70.5};
        double[] y = {0.0, -6.0, 5.0, 6.5, 0.0};
        Vector<point> ob = new Vector<>();
        ob.add(new point(20,10));
        ob.add(new point(30,5));
        ob.add(new point(30,7));
        ob.add(new point(35,8));
        ob.add(new point(50,3));

        Spline2D spline = new Spline2D(x,y);
        spline.generate_target_course();
        frenetPlan n = new frenetPlan();

        double s0 = 0;
        double c_speed = 0;
        double c_d = 0;
        double c_dd = 0;
        double c_ddd = 0;
        Jpolt jp = new Jpolt();
        JpoltCurve L1 = new JpoltCurve("L1", Color.BLUE);
        JpoltCurve L2 = new JpoltCurve("L2", Color.green);
        JpoltCurve L3 = new JpoltCurve("L3", Color.RED);
        JpoltCurve L4 = new JpoltCurve("L4", Color.BLUE);
        JpoltCurve L5 = new JpoltCurve("L5", Color.BLACK);
        Vector<JpoltCurve> Ls = new Vector<>();
        for (int j = 0; j < spline.rx.size(); j++) {
            L1.addPoint(spline.rx.get(j), spline.ry.get(j)+frenetConst.MAX_ROAD_WIDTH);
            L2.addPoint(spline.rx.get(j), spline.ry.get(j));
            L4.addPoint(spline.rx.get(j), spline.ry.get(j)-frenetConst.MAX_ROAD_WIDTH);
        }
        for (int i = 0; i < ob.size(); i++) {
            L5.addPoint(ob.get(i).x,ob.get(i).y);
        }
        L3.VisibleLine = false;
        L5.VisibleLine = false;
        jp.addCurve(L5);
        jp.addCurve(L3);
        jp.addCurve(L4);
        jp.addCurve(L2);
        jp.addCurve(L1);
        //----------------------------------------------------
        Vector<frenetPath> temp = null;
        frenetPath fp = null;
        //----------------------------------------------------
        for (int i = 0; i < 500; i++) {
            temp = n.planning(spline,s0,c_speed,c_d,c_dd,c_ddd,ob);
            fp = n.best_path(temp);
            for (int j = 0; j < fp.x.size(); j++) {
               L3.addPoint(fp.x.get(j),fp.y.get(j));
            }
            s0 = fp.s.get(1);
            c_d = fp.d.get(1);
            c_dd = fp.d_d.get(1);
            c_ddd = fp.d_dd.get(1);
            c_speed = fp.s_d.get(1);


            //jp.setRange(-0.6,5.3,1,-0.2,4.3,0.5);
            try {
                // 暂停 10 秒（10000 毫秒）
                Thread.sleep(100);
                L3.clearPoints();
            } catch (InterruptedException e) {
                e.printStackTrace();
            }

        }
    }
}